#include "mainwindow.h"

//using namespace youbot;

MainWindow::MainWindow(int argc, char** argv) : QMainWindow(),ui(new Ui::MainWindow)
{
  ui->setupUi(this);
  log("Welcome!");

  n = new ros::NodeHandle("gui");
  ROS_INFO("Connected to roscore");

  client_base = NULL;
  client_base = new move_youbot_base::MoveYoubotBaseClient(argc,argv);
  if(client_base != NULL) ROS_INFO("Connected to MoveYoubotBaseServer");
  else ROS_ERROR("Connection to MoveYoubotBaseServer failed");

  client_arm = NULL;
  client_arm = new move_youbot_arm::MoveYoubotArmClient(argc,argv);
  if(client_arm != NULL) ROS_INFO("Connected to MoveYoubotArmServer");
  else ROS_ERROR("Connection to MoveYoubotArmServer failed");

  lastState_base = NULL;
  lastState_arm = NULL;
  timer1.setInterval(250);
  connect(&timer1,SIGNAL(timeout()),this,SLOT(getState()));
}

void MainWindow::log(QString msg)
{

    ui->logger->insertItem(ui->logger->count(),QTime::currentTime().toString()+": "+msg);
    ui->logger->scrollToBottom();
}

void MainWindow::getState()
{
#if TESTING == 1
  actionlib::SimpleClientGoalState *state_base;
  *state_base = client_base->getState();
  if( lastState_base == NULL || *state_base != *lastState_base )
  {
    *lastState_base = *state_base;
    log("[Base] State changed to '"+QString::fromStdString(state_base->toString())+"'");
    double move_x = 0, move_y = 0, move_theta = 0;
    client_base->getCurrentPosition(&move_x,&move_y,&move_theta);

    log("[Base] Actual position is ("+QString::number(move_x)+"m,"
        +QString::number(move_y)+"m,"
        +QString::number(move_theta)+QChar(0x00B0)+")");
  }

  actionlib::SimpleClientGoalState *state_arm;
  *state_arm = client_arm->getState();
  if( lastState_arm == NULL || *state_arm != *lastState_arm )
  {
    *lastState_arm = *state_arm;
    log("[Arm] State changed to '"+QString::fromStdString(state_arm->toString())+"'");
    double j1, j2, j3, j4, j5,m;

    client_arm->getCurrentPosition(&j1,&j2,&j3,&j4,&j5,&g);

    log("[Arm] Actual position is ("+QString::number(j1)+QChar(0x00B0)+","
        +QString::number(j2)+QChar(0x00B0)+","
        +QString::number(j3)+QChar(0x00B0)+","
        +QString::number(j4)+QChar(0x00B0)+","
        +QString::number(j5)+QChar(0x00B0)+","
        +QString::number(g)+"m).");
  }


  if(lastState_base->isDone())timer1.stop();
#else
  log("Not trying to get state -> getState() doesn't work stable so far");
  timer1.stop();
#endif
}


bool MainWindow::simpleBaseTest()
{

    try {
            /*
             * Simple sequence of commands to the youBot:
             */

            if (youBotHasBase)
            {

                    log("Base test not yet implemented.");

                    return true;
            }
            else log("No base available.");

    }
    catch(std::exception& e)
    {
            log(e.what());
            log("unhandled exception");
    }
    return false;
}

bool MainWindow::simpleArmTest()
{
    try
    {
            if (youBotHasArm)
            {
                log("Arm test not yet implemented.");

                return true;
            }
            else log("No arm available.");
    }
    catch(std::exception& e)
    {
            log(e.what());
            log("unhandled exception");
    }

    return false;
}

MainWindow::~MainWindow()
{
    log("Bye bye!");


//!Todo: {disconnect, shutdown, delete} the ROS node "n" here!


    delete ui;
}


void MainWindow::on_pb_test_clicked()
{
    if(ui->rb_arm->isChecked()) simpleArmTest();
    else if(ui->rb_base->isChecked()) simpleBaseTest();
    else log("Choose kind of test!");
}

void MainWindow::on_pb_movebase_clicked()
{
    double move_x = ((double)ui->dial_x->value())/100;
    double move_y = ((double)ui->dial_y->value())/100;
    double move_theta = ui->dial_theta->value();
    double move_velocity = ui->dial_velocity->value();


    client_base->move(move_x,move_y,move_theta,move_velocity);
    timer1.start();
    log("[Base] Goal sent.");


//// Send goal an wait for result -> blocking

//    int move_timeout = ui->hs_timeout->value();
//    if( client->move(move_x,move_y,move_theta,move_velocity,ros::Duration(move_timeout, 0)) )
//      log("Moved base by ("+QString::number(move_x)+"m,"
//                        +QString::number(move_y)+"m,"
//                        +QString::number(move_theta)+QChar(0x00B0)+","
//                        +QString::number(move_velocity)+"%)");
//    else log("Moving base failed!");

//    client->getCurrentPosition(&move_x,&move_y,&move_theta);

//    log("Actual position is ("+QString::number(move_x)+"m,"
//        +QString::number(move_y)+"m,"
//        +QString::number(move_theta)+QChar(0x00B0)+")");

}

void MainWindow::on_pb_toggle_sign_clicked()//Toggles the sign of X/Y coordinates for easy forward and backward moving
{
    ui->dial_x->setValue(ui->dial_x->value()*-1);
    ui->dial_y->setValue(ui->dial_y->value()*-1);
}

void MainWindow::on_pb_stop_clicked()
{
  double move_x,move_y,move_theta;
  log("[Base] Stopping all goals currently running on the action server.");
  client_base->stop();
  client_base->getCurrentPosition(&move_x,&move_y,&move_theta);

  log("[Base] Stopped. Actual position is ("+QString::number(move_x)+"m,"
      +QString::number(move_y)+"m,"
      +QString::number(move_theta)+QChar(0x00B0)+")");
}

void MainWindow::on_pb_zero_clicked()
{
    ui->dial_x->setValue(0);
    ui->dial_y->setValue(0);
    ui->dial_velocity->setValue(50);
    ui->dial_theta->setValue(0);
}

void MainWindow::on_pb_stop_arm_clicked()
{
  log("[Arm] Stopping all goals currently running on the action server.");
  client_arm->stop();
}

void MainWindow::on_pb_zero_arm_clicked()
{
  ui->dial_j1->setValue(0);
  ui->dial_j2->setValue(0);
  ui->dial_j3->setValue(0);
  ui->dial_j4->setValue(0);
  ui->dial_j5->setValue(0);
  ui->dial_gripper->setValue(0);
}

void MainWindow::on_pb_movearm_clicked()
{
    float j1,j2,j3,j4,j5,gripper;

    j1=ui->dial_j1->value();
    j2=ui->dial_j2->value();
    j3=ui->dial_j3->value();
    j4=ui->dial_j4->value();
    j5=ui->dial_j5->value();

    gripper = (float)ui->dial_gripper->value()/100;//convert from cm to m

    client_arm->move(j1,j2,j3,j4,j5,gripper);
    log("[Arm] Goal sent.");
    timer1.start();
}
